% 验证状态转移矩阵

clc;
clear;
load('mcsim', 'in');
par.g = 9.7934752114984;
cst = load('earthconstants_WGS84.mat', 'RE');
cst.deg2Rad = pi / 180;
cst.sec2Rad = pi / 3600 / 180;
in.curSimParVal(1:36, 1) = in.parRefVal(1:36, 2);

%% 生成轨迹
% 常量
cst_trajGen = load('earthconstants_WGS84.mat', 'omegaIE', 'RE', 'epsilonSq');
% 配置
cfg_trajGen.enableTrajectoryGen = true;
cfg_trajGen.enableSensErr = true; % 在轨迹中引入器件误差
cfg_trajGen.enableSensRandErr = false;
cfg_trajGen.enableNavigation = false;
cfg_trajGen.ignore2OdSensErr = true;
cfg_trajGen.IMUOutFilePath = [];
cfg_trajGen.quantizeIMUOut = false;
cfg_trajGen.IMUOutPrecision = '%.16e';
% 参量
par_trajGen.samplePeriod = 0.01;
par_trajGen.g = par.g; % 重力加速度，m/s^2
par_trajGen.IMU.acc.B = in.curSimParVal(10:12, 1) * par_trajGen.g * 1e-6; % 转换前单位μg，转换后单位m/s^2
par_trajGen.IMU.acc.SF0 = ones(3, 1);
par_trajGen.IMU.acc.dSF = in.curSimParVal(13:15, 1) * 1e-6;
par_trajGen.IMU.acc.MA = vec2offdiag(in.curSimParVal(16:21, 1)) * 1e-6; % 转换前单位μrad，转换后单位rad
par_trajGen.IMU.acc.SO = in.curSimParVal(22:24, 1) * 1e-6 / par_trajGen.g; % 转换前单位μg/g^2，转换后单位1/(m/s^2)
par_trajGen.IMU.gyro.B = in.curSimParVal(25:27, 1) * cst.sec2Rad; % 转换前单位°/h，转换后单位rad/s
par_trajGen.IMU.gyro.SF0 = ones(3, 1);
par_trajGen.IMU.gyro.dSFp = in.curSimParVal(28:30, 1) * 1e-6;
par_trajGen.IMU.gyro.dSFn = par_trajGen.IMU.gyro.dSFp;
par_trajGen.IMU.gyro.MA = vec2offdiag(in.curSimParVal(31:36, 1)) * 1e-6; % 转换前单位μrad，转换后单位rad
par_trajGen.IMU.gyro.GS = zeros(3);
% 输入量
in_trajGen.genFuncName = 'gentgi_navtest_regular';
in_trajGen.genFuncInArg = {};
% 初始条件
iniCon_trajGen.CBG = eye(3); % 初始姿态矩阵
iniCon_trajGen.vG = zeros(3, 1); % 初始速度
iniCon_trajGen.pos = [30 * cst.deg2Rad, 110 * cst.deg2Rad, 0]'; % 纬度、经度、高度
% 生成轨迹
[out_trajGen, ~] = gentrajectory(in_trajGen, cfg_trajGen, par_trajGen, iniCon_trajGen, cst_trajGen);

% 运行组合导航
for i=1:5
    % 常量
    cst_intNav = load('earthconstants_WGS84.mat', 'RE', 'e', 'mu', 'J2', 'J3', 'omegaIE', 'epsilonSq');
    % 配置
    cfg_intNav.enableKalmFilt = false;
    cfg_intNav.enableKFUpdate = false;
    cfg_intNav.extrapAlgo = KalmFiltExtrapAlgo.ITER;
    cfg_intNav.updAlgo = KalmFiltUpdAlgo.JOSEPH;
    cfg_intNav.useDCMInAttCalc = false;
    cfg_intNav.dynaMatINSSysBlkForm = 1;
    cfg_intNav.navCoordType = NavCoordType.WAN_AZM;
    switch i
        case 1
            navParErrFigName = '导航参数误差观测值与量测值(ITER外推+φ+游动方位)';
            navParResidErrFigName = '导航参数误差量测残差(ITER外推+φ+游动方位)';
            navParRelatErrFigName = '导航参数误差观测值与量测值相对误差(ITER外推+φ+游动方位)';
            accParResidErrFigName = '加速度计参数(ITER外推+φ+游动方位)';
            gyroParResidErrFigName = '陀螺参数(ITER外推+φ+游动方位)';
            figName = '(ITER外推+φ+游动方位)';
        case 2
            cfg_intNav.extrapAlgo = KalmFiltExtrapAlgo.NORMAL;
            navParErrFigName = '导航参数误差观测值与量测值(NORMAL外推+φ+游动方位)';
            navParResidErrFigName = '导航参数误差量测残差(NORMAL外推+φ+游动方位)';
            navParRelatErrFigName = '导航参数误差观测值与量测值相对误差(NORMAL外推+φ+游动方位)';
            accParResidErrFigName = '加速度计参数(NORMAL外推+φ+游动方位)';
            gyroParResidErrFigName = '陀螺参数(NORMAL外推+φ+游动方位)';
            figName = '(NORMAL外推+φ+游动方位)';
        case 3
            cfg_intNav.dynaMatINSSysBlkForm = 2;
            navParErrFigName = '导航参数误差观测值与量测值(ITER外推+ψ+游动方位)';
            navParResidErrFigName = '导航参数误差量测残差(ITER外推+ψ+游动方位)';
            navParRelatErrFigName = '导航参数误差观测值与量测值相对误差(ITER外推+ψ+游动方位)';
            accParResidErrFigName = '加速度计参数(ITER外推+ψ+游动方位)';
            gyroParResidErrFigName = '陀螺参数(ITER外推+ψ+游动方位)';
            figName = '(ITER外推+ψ+游动方位)';
        case 4
            cfg_intNav.navCoordType = NavCoordType.FREE_AZM;
            navParErrFigName = '导航参数误差观测值与量测值(ITER外推+φ+ 自由方位)';
            navParResidErrFigName = '导航参数误差量测残差(ITER外推+φ+自由方位)';
            navParRelatErrFigName = '导航参数误差观测值与量测值相对误差(ITER外推+φ+自由方位)';
            accParResidErrFigName = '加速度计参数(ITER外推+φ+自由方位)';
            gyroParResidErrFigName = '陀螺参数(ITER外推+φ+自由方位)';
            figName = '(ITER外推+φ+自由方位)';
        otherwise
            cfg_intNav.navCoordType = NavCoordType.GEO;
            navParErrFigName = '导航参数误差观测值与量测值(ITER外推+φ+ENU地理)';
            navParResidErrFigName = '导航参数误差量测残差(ITER外推+φ+ENU地理)';
            navParRelatErrFigName = '导航参数误差观测值与量测值相对误差(ITER外推+φ+ENU地理)';
            accParResidErrFigName = '加速度计参数(ITER外推+φ+ENU地理)';
            gyroParResidErrFigName = '陀螺参数(ITER外推+φ+ENU地理)';
            figName = '(ITER外推+φ+ENU地理)';
    end
    cfg_intNav.sysStateMask = [true(1, 10) false(1, 1)];
    cfg_intNav.sensStateMask = [true(1, 27) false(1, 9)];
    cfg_intNav.enableNumCondCtrl = false; % 关闭数值条件控制
    % 参量
    par_intNav.tBgn = out_trajGen.t(1);
    par_intNav.tEnd = out_trajGen.t(end);
    par_intNav.dT = par_trajGen.samplePeriod;
    par_intNav.tUpd = 1; % Aid组合导航的量测更新周期
    par_intNav.g = par_trajGen.g;
    par_intNav.INSCycPerKalFiltExtrapCyc = int32(10);
    par_intNav.GammaMRGammaMT = diag(zeros(1, 37));
    par_intNav.INSSensRWC = zeros(6, 1);
    par_intNav.INSSensStateProcNoisePSDFull = zeros(36, 1);
    par_intNav.PDiagMin = NaN(nnz([cfg_intNav.sysStateMask cfg_intNav.sensStateMask]), 1);
    par_intNav.PDiagMax = NaN(nnz([cfg_intNav.sysStateMask cfg_intNav.sensStateMask]), 1);
    % 初始条件
    iniCon_intNav.pos = out_trajGen.pos(1, :)'; % 无误差
    iniCon_intNav.vN = out_trajGen.vG(1, :)'; % 无误差
    iniCon_intNav.CBN = out_trajGen.CBG(:, :, 1); % 无误差
    iniCon_intNav.kalmFilt.x = [-in.curSimParVal(1)/cst.RE; in.curSimParVal(2)/cst.RE; in.curSimParVal(3); in.curSimParVal(6:-1:4); in.curSimParVal(9:-1:7)*cst.sec2Rad; in.curSimParVal(2)/cst.RE*tan(out_trajGen.pos(1, 1));...
        in.curSimParVal(10:12)*par_intNav.g*1e-6; in.curSimParVal(13:21)*1e-6; in.curSimParVal(22:24)/par_intNav.g*1e-6; in.curSimParVal(25:27)*cst.sec2Rad; in.curSimParVal(28:36)*1e-6];
    iniCon_intNav.kalmFilt.P = iniCon_intNav.kalmFilt.x*iniCon_intNav.kalmFilt.x';
    % 输入量
    m = size(out_trajGen.accOutErrFree, 1);
    in_intNav.INS.accOut = out_trajGen.accOutErrFree;
    in_intNav.INS.gyroOut = out_trajGen.gyroOutErrFree;
    in_intNav.aid.CNE = zeros(3, 3, m);
    in_intNav.aid.vN = zeros(m, 3);
    in_intNav.aid.vG = out_trajGen.vG;
    in_intNav.aid.CBN = zeros(3, 3, m);
    in_intNav.aid.h = out_trajGen.pos(:, 3);
    in_intNav.aid.alpha = zeros(m, 1);
    in_intNav.traj.CNE = zeros(3, 3, m);
    in_intNav.traj.pos = out_trajGen.pos;
    in_intNav.traj.vN = out_trajGen.vG;
    in_intNav.traj.CBN = zeros(3, 3, m);
    in_intNav.traj.fB = out_trajGen.fBTrue;
    in_intNav.traj.omegaIBB = out_trajGen.omegaIBBTrue;
    in_intNav.traj.FCN = zeros(3, 3, m);
    in_intNav.traj.gN = zeros(m, 3);
    in_intNav.u = NaN(0, 1);
    % 运行导航
    clear('intnav_kfvld');
    % 纯惯导解算(TODO: 因为轨迹发生器无vN, CNE, CBN, FCN, gN, alpha参数输出，故利用纯惯导解算获取这些参数，后续待轨迹发生器完善之后可删除此段纯惯导解算代码)
    [out_inertNavIdeal, ~] = intnav_kfvld(in_intNav, cfg_intNav, par_intNav, iniCon_intNav, cst_intNav);
    % 组合导航
    cfg_intNav.enableKalmFilt = true;
    if cfg_intNav.dynaMatINSSysBlkForm == 2
        dtheta = [iniCon_intNav.kalmFilt.x(1:2, 1); iniCon_intNav.kalmFilt.x(10, 1)];
        iniCon_intNav.kalmFilt.x(4:6, 1) = iniCon_intNav.kalmFilt.x(4:6, 1) + cross(dtheta, out_inertNavIdeal.vN(1, :)');
        iniCon_intNav.kalmFilt.x(7:9, 1) = iniCon_intNav.kalmFilt.x(7:9, 1) - dtheta;
        iniCon_intNav.kalmFilt.P = iniCon_intNav.kalmFilt.x*iniCon_intNav.kalmFilt.x';
    end
    [dL, dlambda, ~] = dposvec2dllwa(in.curSimParVal(2, 1), in.curSimParVal(1, 1), NaN, out_trajGen.pos(1, 1), out_inertNavIdeal.alpha(1, 1), out_inertNavIdeal.CNE(:, :, 1), out_trajGen.pos(1, 3), cst_intNav.RE, cst_intNav.e);
    iniCon_intNav.pos = out_trajGen.pos(1, :)' + [dL; dlambda; in.curSimParVal(3, 1)];
    iniCon_intNav.vN = out_trajGen.vG(1, :)' + in.curSimParVal(6:-1:4);
    iniCon_intNav.CBN = angle2dcm(in.curSimParVal(7)*cst.sec2Rad, in.curSimParVal(8)*cst.sec2Rad, in.curSimParVal(9)*cst.sec2Rad) * out_trajGen.CBG(:, :, 1);
    in_intNav.INS.accOut = out_trajGen.accOut;
    in_intNav.INS.gyroOut = out_trajGen.gyroOut;
    in_intNav.aid.CNE = out_inertNavIdeal.CNE;
    in_intNav.aid.vN = out_inertNavIdeal.vN;
    in_intNav.aid.CBN = out_inertNavIdeal.CBN;
    in_intNav.aid.alpha = out_inertNavIdeal.alpha;
    in_intNav.traj.CNE = out_inertNavIdeal.CNE;
    in_intNav.traj.CBN = out_inertNavIdeal.CBN;
    in_intNav.traj.fB = out_trajGen.fBTrue;
    in_intNav.traj.omegaIBB = out_trajGen.omegaIBBTrue;
    in_intNav.traj.FCN = out_inertNavIdeal.FCN;
    in_intNav.traj.gN = out_inertNavIdeal.gN;
    [out_intNavError, auxOut_intNavError] = intnav_kfvld(in_intNav, cfg_intNav, par_intNav, iniCon_intNav, cst_intNav);
    
    %% 后处理
    H = eye(37);
    zHat = (H*out_intNavError.kalmFilt_x')';
    zErr = auxOut_intNavError.kalmFilt_zObs - zHat;
    structNavPar(1) = navparerrarr2struct(auxOut_intNavError.kalmFilt_zObs(:, 1:10), [1:10 NaN(1, 5)], false);
    structNavPar(2) = navparerrarr2struct(zHat(:, 1:10), [1:10 NaN(1, 5)], false);
    plotnavparerr(out_intNavError.kalmFilt_tExtrap, structNavPar, {'观测值', '量测值'}, [], [], [], navParErrFigName);
    disp(navParErrFigName);
    snapnow;
    structNavParResid = navparerrarr2struct(zErr(:, 1:10), [1:10 NaN(1, 5)], false);
    plotnavparerr(out_intNavError.kalmFilt_tExtrap, structNavParResid, {'量测残差'}, [], [], [], navParResidErrFigName);
    disp(navParResidErrFigName);
    snapnow;
    structNavParRelat = navparerrarr2struct(relatdiff(auxOut_intNavError.kalmFilt_zObs(:, 1:10), zHat(:, 1:10), false, 1, 20), [1:10 NaN(1, 5)], false);
    plotnavparerr(out_intNavError.kalmFilt_tExtrap, structNavParRelat, {'相对误差'}, true, [], [], navParRelatErrFigName);
    disp(navParRelatErrFigName);
    snapnow;
    % 惯性器件量测残差
    structAccPar = acctriadpararr2struct(zErr(:, 11:25), [NaN(1, 3) 1:15], false);
    plotacctriadpar(out_intNavError.kalmFilt_tExtrap, structAccPar, {'加速度计参数量测残差'}, [], [], accParResidErrFigName);
    disp(accParResidErrFigName);
    snapnow;
    structGyroPar = gyrotriadpararr2struct(zErr(:, 26:37), [NaN(1, 3) 1:12 NaN(1, 9)], false);
    plotgyrotriadpar(out_intNavError.kalmFilt_tExtrap, structGyroPar, {'陀螺参数量测残差'}, [], [], gyroParResidErrFigName);
    disp(gyroParResidErrFigName);
    snapnow;
    % 验证协方差矩阵的外推算法
    [p, q] = size(out_intNavError.kalmFilt_x);
    xxT = NaN(q, q, p);
    for j=1:p
        xxT(:, :, j) =  out_intNavError.kalmFilt_x(j, :)'*out_intNavError.kalmFilt_x(j, :);
    end
    PErr = auxOut_intNavError.kalmFilt_P - xxT;
    PRelatErr = relatdiff(auxOut_intNavError.kalmFilt_P, xxT);
    PErr_Max = reshape(max(max(abs(PErr))), p, 1);
    PRelatErr_Max = reshape(max(max(abs(PRelatErr))), p, 1);
    preparefig(['误差协方差矩阵外推算法误差' figName]);
    subplot(2, 1, 1)
    plot(out_intNavError.kalmFilt_tExtrap, PErr_Max);
    title(['P阵各元素的绝对误差的最大值' figName]);
    xlabel('时间t');
    subplot(2, 1, 2)
    plot(out_intNavError.kalmFilt_tExtrap, PRelatErr_Max);
    title(['P阵各元素的相对误差的最大值' figName]);
    xlabel('时间t');
    ylabel('无单位');
end